/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates;

import distance.IRSensor;
import distance.DistanceSensor;
import distance.UltrasonicSensor;
import edu.wpi.first.wpilibj.AnalogChannel;
import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.camera.AxisCamera;

/**
 *
 * @author 708
 */
public class RobotParts {
    //array holding motor objects
    private Object[] motors = {
            //window1
            new Relay(IOConstants.crioSlots.DigitalSideCar1,IOConstants.RelayChannels.Spike_windowMotor1),
            //window2
            new Relay(IOConstants.crioSlots.DigitalSideCar1,IOConstants.RelayChannels.Spike_windowMotor2),
            //CIM1
            new Victor(IOConstants.crioSlots.DigitalSideCar2,IOConstants.PWMChannels.Vic_CIM1),
            //Bane1
            new Victor(IOConstants.crioSlots.DigitalSideCar2,IOConstants.PWMChannels.Vic_Banebots1),
            //Bane2
            new Jaguar(IOConstants.crioSlots.DigitalSideCar2,IOConstants.PWMChannels.Jag_Banebots2),
            //CIM2
            new EncoderMotor(IOConstants.DigitalIOChannels.encoderA,
                    IOConstants.crioSlots.DigitalSideCar2,
                    IOConstants.DigitalIOChannels.encoderB,
                    IOConstants.crioSlots.DigitalSideCar2,
                    new Victor(IOConstants.crioSlots.DigitalSideCar2,IOConstants.PWMChannels.Vic_CIM2),
                    400
                    ),
            //CIM3
            new Victor(IOConstants.crioSlots.DigitalSideCar1,IOConstants.PWMChannels.Vic_CIM3),
            //Bane3
            new Victor(IOConstants.crioSlots.DigitalSideCar1,IOConstants.PWMChannels.Vic_Banebots3),
            //Bane4
            new Victor(IOConstants.crioSlots.DigitalSideCar1,IOConstants.PWMChannels.Vic_Banebots4),
            //CIM4
            new Victor(IOConstants.crioSlots.DigitalSideCar1,IOConstants.PWMChannels.Vic_CIM4)
        };
    
    //motor indexes
    public static final int window1 = 0;
    public static final int window2 = 1;
    public static final int CIM1 = 2;
    public static final int Bane1 = 3;
    public static final int Bane2 = 4;
    public static final int CIM2 = 5;
    public static final int CIM3 = 6;
    public static final int Bane3 = 7;
    public static final int Bane4 = 8;
    public static final int CIM4 = 9;

    private RPMEncoder testEncoder;     //for testing the shooter
    private AnalogChannel potentiometer;
    private DistanceSensor ultrasonic1;
    private DistanceSensor ultrasonic2;
    private DistanceSensor IR1;
    private DistanceSensor IR2;
    private DigitalInput limitSwitch;
    private Compressor compressor;
    private Gyro gyro;
    private AxisCamera camera;
    
    public RobotParts(){
        //initialize parts here
        gyro = new Gyro(new AnalogChannel(IOConstants.AnalogInputChannels.gyro));
        limitSwitch = new DigitalInput(IOConstants.DigitalIOChannels.limitSwitch);
        potentiometer = new AnalogChannel(IOConstants.AnalogInputChannels.potentiometer);
        ultrasonic1 = new UltrasonicSensor(IOConstants.AnalogInputChannels.ultrasonic1,UltrasonicSensor.MB1010);
        ultrasonic2 = new UltrasonicSensor(IOConstants.AnalogInputChannels.ultrasonic2,UltrasonicSensor.MB1340);
        IR1 = new IRSensor(IOConstants.AnalogInputChannels.IR1,IRSensor.GP2Y0A02YK0F);
        IR2 = new IRSensor(IOConstants.AnalogInputChannels.IR2,IRSensor.GP2Y0A21YK0F);

        testEncoder = new RPMEncoder(IOConstants.crioSlots.DigitalSideCar2,IOConstants.DigitalIOChannels.shooterEncA,
                IOConstants.crioSlots.DigitalSideCar2,IOConstants.DigitalIOChannels.shooterEncB,60.0);

        //encoder will read in revolutions/sec, which will be multiplied by 60 for rpms
        testEncoder.setDistancePerPulse(1.0/250.0);
        testEncoder.start();

        //setup camera
        camera = AxisCamera.getInstance("10.7.8.11");
        camera.writeResolution(AxisCamera.ResolutionT.k320x240);
        camera.writeColorLevel(100);
        camera.writeBrightness(50);
        camera.writeCompression(30);
    }
    
    /**
     * Gets the motor object specified by an array index.
     * @param index
     * @return 
     */
    public Object getMotor(int index){
        if(index >= 0 && index < motors.length){
            return motors[index];
        }else{
            return null;
        }
    }
    
    public void setMotor(int index,double speed){
            Object o = motors[index];
            
            if(o != null){
                if(o instanceof SpeedController){
                    SpeedController s = (SpeedController)o;
                    s.set(speed);
                }else if(o instanceof Relay){
                    Relay r = (Relay)o;
                    if(Math.abs(speed) <= .25){
                        r.set(Relay.Value.kOff);
                    }else if(speed > 0){
                        r.set(Relay.Value.kForward);
                    }else if(speed < 0){
                        r.set(Relay.Value.kReverse);
                    }
                }
            }else{
                System.out.println("Non-existent motor accessed.");
            }
    }

    public RPMEncoder getTestEncoder(){
        return testEncoder;
    }

    public AxisCamera getCamera(){
        return camera;
    }
    
    public Gyro getGyro(){
        return gyro;
    }

    public DistanceSensor getIR1() {
        return IR1;
    }

    public DistanceSensor getIR2(){
        return IR2;
    }

    public DistanceSensor getUltrasonic1() {
        return ultrasonic1;
    }

    public DistanceSensor getUltrasonic2() {
        return ultrasonic2;
    }



    public DigitalInput getLSwitch(){
        return limitSwitch;
    }
    
    public AnalogChannel getPotentiometer(){
        return potentiometer;
    }
    
    public int getTotalMotors(){
        return motors.length;
    }
    
    public void stopMotors(){
        for(int i = 0; i < motors.length; i++){
            setMotor(i,0);
        }
    }
}
